#include "can_controller.h"
#include "remote.h"


#if defined GIMBAL_BOARD_UP ||	defined GIMBAL_BOARD_DOWN
#include "heat_control.h"
#endif

#ifdef CHASSIS_BOARD
#include "power_control.h"
#include "ros.h"
#include "referee.h"
#endif
		

#ifdef  GIMBAL_BOARD_DOWN

#define CAN_TRIGGER_MOTOR_ID    			0x203		//CAN1
#define	CAN_PITCH_MOTOR_ID      			0x205       //CAN1 
#define CAN_4310_YAW_MASTER_ID  			0x301		//CAN1
#define CAN_Refereedata2Gimbal   			0x209		//CAN2
#define CAN_REMOTE_RC_S						0x210		//CAN2
#define CAN_REMOTE_RC_C						0x211		//CAN2
#define CAN_REMOTE_RC_M					  	0x212		//CAN2

#define PITCH_OFFSET_ANGLE_DOWN 			7500
#endif

#ifdef GIMBAL_BOARD_UP
#define	CAN_4310_YAW_MASTER_ID				0x302	//CAN1  canid为0x002
#define CAN_TRIGGER_MOTOR_ID   				0x203	//CAN1
#define	CAN_PITCH_MOTOR_ID      			0x205	//CAN1
#define CAN_Visondata2Chassis     			0x20A	//CAN2 	传给导航数据
#define CAN_Refereedata2Gimbal   			0x209
#define CAN_REMOTE_RC_S						0x210
#define CAN_REMOTE_RC_C						0x211
#define CAN_REMOTE_RC_M					  	0x212	

#define PITCH_OFFSET_ANGLE_UP   10 
#endif

#ifdef CHASSIS_BOARD
#define	CAN_CHASSIS_MOTOR_LU_ID 		0x201  //右前
#define	CAN_CHASSIS_MOTOR_LD_ID 		0x202  //左前
#define	CAN_CHASSIS_MOTOR_RD_ID 		0x203  //左后
#define	CAN_CHASSIS_MOTOR_RU_ID 		0x204  //右后
#define CAN_Visondata2Chassis       	0x20A
//哨兵未用到，加入超级电容备用
// #define	CAN_SEND_DIGITAL_POWER_ID		0x210
// #define	CAN_DIGITAL_POWER_ID 			0x211
#endif

//电机反馈数据结构体

motor_measure_t  motor_param_get[NUM_OF_MOTOR] = {0};

// Motor_Inf dm4310_message;
// motor_measure_DM  motor_get_DM;







#if defined GIMBAL_BOARD_UP ||	defined GIMBAL_BOARD_DOWN

void Get_referee_shootdata(referee_shoot_data_t *ptr,uint8_t arr[]);
#endif

#ifdef CHASSIS_BOARD
void CAN_Get_Digital_Power(digital_power_t *ptr, uint8_t CAN_RX_date[]);
void CAN_Get_Vison(referee_flag_t  *ptr, uint8_t CAN_RX_date[]);
#endif


void Get_Moto_Date_DM4310(uint8_t ptr[],Motor_Inf *message);
void Get_Moto_Measure_DM4310(motor_measure_DM *ptr,Motor_Inf DM_Motor);
void Get_Motor_Measure_4310(motor_measure_t *ptr,uint8_t arr[]);
void Get_Moto_Measure_3508(motor_measure_t *ptr,uint8_t arr[]);
void Get_Motor_Measure_2006(motor_measure_t *ptr,uint8_t arr[]);
void Get_Motor_Measure_6020(motor_measure_t *ptr,uint8_t arr[]);
void Get_Motor_Offset(motor_measure_t *ptr,uint8_t arr[]);


//===================================================================================================================//
/**
* @brief					CAN?????§Ø???????
	* @content 			CAN1????---??????????? + ??????????
									CAN2????
									
  * @param[out]		void
  * @param[in]		CAN_HandleTypeDef
  * @retval				void
*/
#ifdef GIMBAL_BOARD_DOWN
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef  CAN1_Rx_Header;
	CAN_RxHeaderTypeDef  CAN2_Rx_Header;
	uint8_t CAN1_RX_date[8];																										
	uint8_t CAN2_RX_date[8];																										

  	if(hcan == &hcan1)																													
	{
		HAL_CAN_GetRxMessage(&hcan1,CAN_RX_FIFO0,&CAN1_Rx_Header,CAN1_RX_date);
		switch(CAN1_Rx_Header.StdId)
		{
			case CAN_TRIGGER_MOTOR_ID:						
			{
				
				if(motor_param_get[TRIGGER_MOTOR].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[TRIGGER_MOTOR],CAN1_RX_date);
					motor_param_get[TRIGGER_MOTOR].msg_cnt++;
				}
				
				else 
				{
					motor_param_get[TRIGGER_MOTOR].msg_cnt = 11;
					Get_Motor_Measure_2006(&motor_param_get[TRIGGER_MOTOR],CAN1_RX_date);
				}
			}break;
			case CAN_PITCH_MOTOR_ID:	
			{						
				if(motor_param_get[PITCH_MOTOR].msg_cnt <= 10)
				{
//					Get_Motor_Offset(&motor_param_get[PITCH_MOTOR],CAN1_RX_date);
					motor_param_get[PITCH_MOTOR].offset_angle=PITCH_OFFSET_ANGLE_DOWN;					
					motor_param_get[PITCH_MOTOR].angle=PITCH_OFFSET_ANGLE_DOWN;
					motor_param_get[PITCH_MOTOR].msg_cnt++;
				}
				else 
				{
					motor_param_get[PITCH_MOTOR].msg_cnt = 11;
					Get_Motor_Measure_6020(&motor_param_get[PITCH_MOTOR],CAN1_RX_date);
				}
			}break;
		}	
	}else if(hcan == &hcan2)																											
	{
		HAL_CAN_GetRxMessage(&hcan2,CAN_RX_FIFO0,&CAN2_Rx_Header,CAN2_RX_date);
		switch(CAN2_Rx_Header.StdId)
		{
			case CAN_4310_YAW_MASTER_ID:
			{
				if(motor_param_get[YAW_MOTOR].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[YAW_MOTOR],CAN1_RX_date);	
					motor_param_get[YAW_MOTOR].msg_cnt++;				
				}
				else 
				{
					Get_Motor_Measure_4310(&motor_param_get[YAW_MOTOR],CAN1_RX_date);	
				}	
			}break;
			case CAN_REMOTE_RC_S:																									
			{
				Get_REMOTEDATA_s(CAN2_RX_date);
			}break;
			case CAN_REMOTE_RC_C:
			{
				Get_REMOTEDATA_Channel(CAN2_RX_date);
			}break;
			case CAN_REMOTE_RC_M:			
			{
				// Get_REMOTEDATA_Mouse(CAN2_RX_date);
			}break;
			// case CAN_YAW_MOTOR_ID:																							
			// {
				
			// 	if(motor_param_get[YAW_MOTOR].msg_cnt <= 10)
			// 	{
			// 		Get_Motor_Offset(&motor_param_get[YAW_MOTOR],CAN2_RX_date);
			// 		motor_param_get[YAW_MOTOR].msg_cnt++;
			// 	}
				
			// 	else 
			// 	{
			// 		motor_param_get[YAW_MOTOR].msg_cnt = 11;
			// 		Get_Motor_Measure_6020(&motor_param_get[YAW_MOTOR],CAN2_RX_date);
			// 	}
			// }break;
			
			 case CAN_Refereedata2Gimbal:
			{
				Get_referee_shootdata(&referee_shoot_data,CAN2_RX_date);
			}break;
			default:break;
		}
	}
}
#endif
#ifdef GIMBAL_BOARD_UP
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef  CAN1_Rx_Header;
	CAN_RxHeaderTypeDef  CAN2_Rx_Header;

 	uint8_t CAN1_RX_date[8];																										
	uint8_t CAN2_RX_date[8];																										
	

  	if(hcan == &hcan1)																													
	{
		HAL_CAN_GetRxMessage(&hcan1,CAN_RX_FIFO0,&CAN1_Rx_Header,CAN1_RX_date);
		switch(CAN1_Rx_Header.StdId)
		{
			case CAN_TRIGGER_MOTOR_ID:						
			{
				
				if(motor_param_get[TRIGGER_MOTOR].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[TRIGGER_MOTOR],CAN1_RX_date);
					motor_param_get[TRIGGER_MOTOR].msg_cnt++;
				}
				
				else 
				{
					motor_param_get[TRIGGER_MOTOR].msg_cnt = 11;
					Get_Motor_Measure_2006(&motor_param_get[TRIGGER_MOTOR],CAN1_RX_date);
				}
			}break;
			case CAN_PITCH_MOTOR_ID:	
			{						
				if(motor_param_get[PITCH_MOTOR].msg_cnt == 0)
				{
//					Get_Motor_Offset(&motor_param_get[PITCH_MOTOR],CAN1_RX_date);
					motor_param_get[PITCH_MOTOR].offset_angle=PITCH_OFFSET_ANGLE_UP;					
					motor_param_get[PITCH_MOTOR].angle=PITCH_OFFSET_ANGLE_UP;
					motor_param_get[PITCH_MOTOR].msg_cnt++;
				}
				else 
				{
					Get_Motor_Measure_6020(&motor_param_get[PITCH_MOTOR],CAN1_RX_date);
				}
			}break;
			case CAN_4310_YAW_MASTER_ID:
			{
				if(motor_param_get[YAW_MOTOR].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[YAW_MOTOR],CAN1_RX_date);
					motor_param_get[YAW_MOTOR].msg_cnt++;					
				}
				else 
				{
					Get_Motor_Measure_4310(&motor_param_get[YAW_MOTOR],CAN1_RX_date);	
				}
			}break;
			// case CAN_DM4310_MOTOR_ID:
			// {
			// 	Get_Moto_Date_DM4310(CAN1_RX_date,&dm4310_message);
			// 	if(motor_get_DM.msg_cnt<= 10)	
			// 	{
			// 		motor_get_DM.msg_cnt++;
			// 		motor_get_DM.angle=dm4310_message.pos;
			// 		motor_get_DM.offset_angle=motor_get_DM.angle;
			// 	}
			// 	else 
			// 	{
			// 		Get_Moto_Measure_DM4310(&motor_get_DM,dm4310_message);				
			// 	}


			//  }break;
		}	
	}else if(hcan == &hcan2)																											
	{
		HAL_CAN_GetRxMessage(&hcan2,CAN_RX_FIFO0,&CAN2_Rx_Header,CAN2_RX_date);
		switch(CAN2_Rx_Header.StdId)
		{
			case CAN_REMOTE_RC_S:																									
			{
				Get_REMOTEDATA_s(CAN2_RX_date);
			}break;
			case CAN_REMOTE_RC_C:
			{
				Get_REMOTEDATA_Channel(CAN2_RX_date);
			}break;
			case CAN_REMOTE_RC_M:			
			{
				// Get_REMOTEDATA_Mouse(CAN2_RX_date);
			}break;
			 case CAN_Refereedata2Gimbal:
			{
				Get_referee_shootdata(&referee_shoot_data,CAN2_RX_date);
			}break;
			default:break;
		}
	}
}
#endif
#ifdef CHASSIS_BOARD
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef  CAN1_Rx_Header;
	CAN_RxHeaderTypeDef  CAN2_Rx_Header;
	uint8_t CAN1_RX_date[8];																										//CAN1总线上接收报文数据变量
	uint8_t CAN2_RX_date[8];																										//CAN2总线上接收报文数据变量
	
  if(hcan == &hcan1)																													//接收底盘电机发送信息
	{
		HAL_CAN_GetRxMessage(&hcan1,CAN_RX_FIFO0,&CAN1_Rx_Header,CAN1_RX_date);
		
		switch(CAN1_Rx_Header.StdId)
		{
			case CAN_CHASSIS_MOTOR_LU_ID:
			{
				if(motor_param_get[CHASSIS_MOTOR_LU].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[CHASSIS_MOTOR_LU],CAN1_RX_date);
					motor_param_get[CHASSIS_MOTOR_LU].msg_cnt++;
				}
				else 
				{
					motor_param_get[CHASSIS_MOTOR_LU].msg_cnt = 11;
					Get_Moto_Measure_3508(&motor_param_get[CHASSIS_MOTOR_LU],CAN1_RX_date);
				}
			}break;
			
			case CAN_CHASSIS_MOTOR_LD_ID:
			{
				if(motor_param_get[CHASSIS_MOTOR_LD].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[CHASSIS_MOTOR_LD],CAN1_RX_date);
					motor_param_get[CHASSIS_MOTOR_LD].msg_cnt++;
				}
				else 
				{
					motor_param_get[CHASSIS_MOTOR_LD].msg_cnt = 11;
					Get_Moto_Measure_3508(&motor_param_get[CHASSIS_MOTOR_LD],CAN1_RX_date);
				}
			}break;
			
			case CAN_CHASSIS_MOTOR_RD_ID:
			{
				if(motor_param_get[CHASSIS_MOTOR_RD].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[CHASSIS_MOTOR_RD],CAN1_RX_date);
					motor_param_get[CHASSIS_MOTOR_RD].msg_cnt++;
				}
				else 
				{
					motor_param_get[CHASSIS_MOTOR_RD].msg_cnt = 11;
					Get_Moto_Measure_3508(&motor_param_get[CHASSIS_MOTOR_RD],CAN1_RX_date);
				}
			}break;
			
			case CAN_CHASSIS_MOTOR_RU_ID:
			{
				if(motor_param_get[CHASSIS_MOTOR_RU].msg_cnt <= 10)
				{
					Get_Motor_Offset(&motor_param_get[CHASSIS_MOTOR_RU],CAN1_RX_date);
					motor_param_get[CHASSIS_MOTOR_RU].msg_cnt++;
				}
				else 
				{
					motor_param_get[CHASSIS_MOTOR_RU].msg_cnt = 11;
					Get_Moto_Measure_3508(&motor_param_get[CHASSIS_MOTOR_RU],CAN1_RX_date);
				}
			}break;
			
			// case CAN_DIGITAL_POWER_ID:
			// {
			// 	CAN_Get_Digital_Power(&digital_power_get, CAN1_RX_date);
			// }break;
			
			default:break;
		}
	}
	else if(hcan == &hcan2)																											//接收云台主控发送信息
	{
		HAL_CAN_GetRxMessage(&hcan2,CAN_RX_FIFO0,&CAN2_Rx_Header,CAN2_RX_date);
		switch(CAN2_Rx_Header.StdId)
		{
			case CAN_Visondata2Chassis:
			{
				CAN_Get_Vison(&referee_flag ,CAN2_RX_date);
			}break;
			
			default:break;			
		}
	}
}
#endif









//===================================================================================================================//
/**********************************************CAN************************************************/
//===================================================================================================================//
/**
* @brief					??????????????
	* @content 			CAN??????????Э?? + ??????motor get?????????????offset_angle????
									
  * @param[out]		motor_measure_t *
  * @param[in]		motor_measure_t *
									uint8_t *
  * @retval				void
*/
void Get_Motor_Offset(motor_measure_t *ptr,uint8_t arr[])
{
	ptr->angle=(uint16_t)(arr[0]<<8 | arr[1]);
	ptr->offset_angle = ptr->angle;
}


/**
  * @brief				???CAN????2006?????????
  * @param[out]		
  * @param[in]		ptr:?????????????
                  CAN_RX_date:?????????CAN???????????
  * @retval				
*/
void Get_Motor_Measure_2006(motor_measure_t *ptr,uint8_t arr[])
{
	ptr->last_angle = ptr->angle;
	ptr->angle = (uint16_t)(arr[0]<<8 | arr[1]) ;
	ptr->speed_rpm  = (int16_t)(arr[2]<<8 | arr[3]);
	ptr->real_current = (int16_t)(arr[4]<<8 | arr[5]);
	
	if(ptr->angle - ptr->last_angle > 4096)
		ptr->round_cnt --;
	else if (ptr->angle - ptr->last_angle < -4096)
		ptr->round_cnt ++;
	ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
}


/**
  * @brief				???CAN????6020?????????
  * @param[out]		
  * @param[in]		ptr:?????????????
                  CAN_RX_date:?????????CAN???????????
  * @retval				
*/
void Get_Motor_Measure_6020(motor_measure_t *ptr,uint8_t arr[])
{
	ptr->last_angle = ptr->angle;
	ptr->angle = (uint16_t)(arr[0]<<8 | arr[1]) ;
	ptr->speed_rpm  = (int16_t)(arr[2]<<8 | arr[3]);
	ptr->real_current = (int16_t)(arr[4]<<8 | arr[5]);
	ptr->temperature = arr[6];

	if(ptr->angle - ptr->last_angle > 4096)
		ptr->round_cnt --;
	else if (ptr->angle - ptr->last_angle < -4096)
		ptr->round_cnt ++;
	ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
	ptr->total_radangle=ptr->total_angle*coder2radian;
}


/**
  * @brief				???CAN????3508?????????
  * @param[out]		
  * @param[in]		ptr:?????????????
                  CAN_RX_date:?????????CAN???????????
  * @retval				
*/
void Get_Moto_Measure_3508(motor_measure_t *ptr,uint8_t CAN_RX_date[])
{
	ptr->last_angle = ptr->angle;
	ptr->angle = (uint16_t)(CAN_RX_date[0]<<8 | CAN_RX_date[1]) ;
	ptr->speed_rpm  = (int16_t)(CAN_RX_date[2]<<8 | CAN_RX_date[3]);
	ptr->real_current = (int16_t)(CAN_RX_date[4]<<8 | CAN_RX_date[5]);
	ptr->temperature = CAN_RX_date[6];
	
	if(ptr->angle - ptr->last_angle > 4096)
		ptr->round_cnt --;
	else if (ptr->angle - ptr->last_angle < -4096)
		ptr->round_cnt ++;
	ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
}


/**
  * @brief				获取CAN通讯的6020电机的返回值
  * @param[out]		
  * @param[in]		ptr:目标数据的内存地址
                  	CAN_RX_date:保存的来自CAN的数据的数组
  * @retval				
*/
void Get_Motor_Measure_4310(motor_measure_t *ptr,uint8_t arr[])
{
	ptr->last_angle = ptr->angle;
	ptr->angle = (uint16_t)(arr[0]<<8 | arr[1]) ;
	ptr->speed_rpm  = (int16_t)(arr[2]<<8 | arr[3]);//放大100倍
	ptr->real_current = (int16_t)(arr[4]<<8 | arr[5]);
	ptr->temperature = arr[6];

	if(ptr->angle - ptr->last_angle > 4096)
		ptr->round_cnt --;
	else if (ptr->angle - ptr->last_angle < -4096)
		ptr->round_cnt ++;
	ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
	ptr->total_radangle=ptr->total_angle*coder2radian;
}

void Get_Moto_Date_DM4310(uint8_t ptr[],Motor_Inf *message)
{
	message->id = (ptr[0])&0x0F;
	message->state = (ptr[0])>>4;
 	message->p_int=(ptr[1]<<8)|ptr[2];
	message->v_int=(ptr[3]<<4)|(ptr[4]>>4);
	message->t_int=((ptr[4]&0xF)<<8)|ptr[5];
	message->pos = uint_to_float(message->p_int, P_MIN, P_MAX, 16); // (-12.5,12.5)
	message->vel = uint_to_float(message->v_int, V_MIN, V_MAX, 12); // (V_MIN,V_MAX)
	message->toq = uint_to_float(message->t_int, T_MIN, T_MAX, 12);  // (-18.0,18.0)
	message->Tmos = (float)(ptr[6]);
	message->Tcoil = (float)(ptr[7]);
}
void Get_Moto_Measure_DM4310(motor_measure_DM *ptr,Motor_Inf DM_Motor)
{
	ptr->angle = DM_Motor.pos; //单位为弧度【rad】一圈6.25
	ptr->speed_rpm  = DM_Motor.vel;  //单位为弧度每秒【rad/s】额定速度120rpm（12.5rad/s），峰值速度200rpm（）
	ptr->temperature =  DM_Motor.state;
	
	
	if(ptr->angle - ptr->last_angle > DM_PI)
		ptr->round_cnt -= 1;
	else if (ptr->angle - ptr->last_angle < -DM_PI)
		ptr->round_cnt += 1;
	
	ptr->total_radangle = ptr->round_cnt * (2*DM_PI) + ptr->angle - ptr->offset_angle;
	ptr->last_angle = ptr->angle;
}


#if defined GIMBAL_BOARD_UP ||	defined GIMBAL_BOARD_DOWN
void Get_referee_shootdata(referee_shoot_data_t *ptr,uint8_t arr[])
{
	ptr->shooter_17mm_1_barrel_heat= arr[0]<<8 | arr[1];
	ptr->shooter_17mm_2_barrel_heat= arr[2]<<8 | arr[3];
	ptr->enemy_outpostarrive_flag=arr[4]<<8 | arr[5];
	ptr->power_management_shooter_output=arr[6]<<8 | arr[7];
}


#endif
#ifdef CHASSIS_BOARD
void CAN_Get_Digital_Power(digital_power_t *ptr, uint8_t CAN_RX_date[])
{
	ptr->cap_voltage_high 						= CAN_RX_date[0];
	ptr->cap_voltage_low						= CAN_RX_date[1];
	ptr->chassis_output_current_high 			= CAN_RX_date[2];
	ptr->chassis_output_current_low  			= CAN_RX_date[3];
	ptr->cap_energy_percent 					= CAN_RX_date[4];
	ptr->cap_status								= CAN_RX_date[5];
	
	ptr->cap_voltage_16							= ((uint16_t)ptr->cap_voltage_high << 8) | ptr->cap_voltage_low;
	ptr->chassis_output_current_16 				= ((uint16_t) ptr->chassis_output_current_high << 8) | ptr->chassis_output_current_low;
	
	ptr->cap_voltage							= ptr->cap_voltage_16 / 100.0f;
	ptr->chassis_output_current 				= ptr->chassis_output_current_16 / 100.0f;
}	


void CAN_Get_Vison(referee_flag_t  *ptr, uint8_t CAN_RX_date[])
{
	referee_flag.vision_State=(uint8_t )(CAN_RX_date[0]<<8 |CAN_RX_date[1]);
	if(Referee_robotrx__all.projectile_allowance.projectile_allowance_17mm<5)
	{
	referee_flag.vision_State=0;		
	}	
	referee_flag.target_distance=CAN_RX_date[2]<<8 |CAN_RX_date[3];
	referee_flag.angle=(CAN_RX_date[4]<<8 |CAN_RX_date[5]);
	//保留位
//	ptr->reserve=CAN_RX_date[6]<<8 |CAN_RX_date[7];
}
#endif